#include "fifo.h"

#define MIN(x, y)   ((x) > (y) ? (y) : (x))

static uint8_t power_of_two(uint32_t size)
{
    return (size != 0 && ((size & (size - 1)) == 0));
}

uint8_t fifo_init(fifo_t *fifo, uint8_t *buffer, uint32_t size)
{
    if ((fifo == NULL) || (buffer == NULL) || (power_of_two(size) == 0))
        return false;

    fifo->buffer = buffer;
    fifo->in = fifo->out = 0;
    fifo->size = size;

    return true;
}

uint32_t fifo_length(fifo_t *fifo)
{
    return (fifo->in - fifo->out);
}


uint32_t fifo_put(fifo_t *fifo, uint8_t *buffer, uint32_t len)
{
    uint32_t l;

    len = MIN(len, fifo->size - fifo_length(fifo));

    l = MIN(len, fifo->size - (fifo->in & (fifo->size - 1)));
    memcpy(fifo->buffer + (fifo->in & (fifo->size - 1)), buffer, l);
    memcpy(fifo->buffer, buffer + l, len - l);

    fifo->in += len;

    return len;
}

uint32_t fifo_get(fifo_t *fifo, uint8_t *buffer, uint32_t len)
{
    uint32_t l;

    len = MIN(len, fifo->in - fifo->out);

    l = MIN(len, fifo->size - (fifo->out & (fifo->size - 1)));
    memcpy(buffer, fifo->buffer + (fifo->out & (fifo->size - 1)), l);
    memcpy(buffer + l, fifo->buffer, len - l);

    fifo->out += len;

    return len;
}
